#include "base.h"

//
// Matrix
//

void mat3_identity(mat3 m)
{
    for(int y = 0; y < 3; y++)
    {
        for(int x = 0; x < 3; x++)
        {
            if(x == y)
            {
                m[x][y] = 1.0;
            }
            else
            {
                m[x][y] = 0.0;
            }
        }
    }
    return;
}

void mat3_copy(mat3 dest, mat3 src)
{
    memcpy(dest, src, sizeof(mat3));
    return;
}

void mat3_translate(mat3 m, vec2 t)
{
    mat3 tmp, result;

    // Build translate matrix
    mat3_identity(tmp);
    tmp[2][0] = t[0];
    tmp[2][1] = t[1];

    // Multiply target matrix
    mat3_mult(result, m, tmp);
    mat3_copy(m, result);
    return;
}

void mat3_scale(mat3 m, vec2 s)
{
    mat3 tmp, result;

    // Build scale matrix
    mat3_identity(tmp);
    tmp[0][0] = s[0];
    tmp[1][1] = s[1];

    // Multiply target matrix
    mat3_mult(result, m, tmp);
    mat3_copy(m, result);
    return;
}

void mat3_rotate(mat3 m, float angle)
{
    mat3 tmp, result;
    angle = angle / 180 * 3.14159265;
    // Build translate matrix
    mat3_identity(tmp);
    const float cos_a = cos(angle);
    const float sin_a = sin(angle);
    tmp[0][0] = cos_a;
    tmp[1][0] = sin_a;
    tmp[0][1] = - sin_a;
    tmp[1][1] = cos_a;

    // Multiply target matrix
    mat3_mult(result, m, tmp);
    mat3_copy(m, result);
    return;
}

void mat3_mult(mat3 dest, mat3 m1, mat3 m2)
{
    // Unoptimised version
    for(int y = 0; y < 3; y++)
    {
        for(int x = 0; x < 3; x++)
        {
            float sum = 0.0;
            for(int k = 0; k < 3; k++)
            {
                sum += m1[x][k] * m2[k][y];
            }
            dest[x][y] = sum;
        }
    }
    return;
}

void mat3_mult_vec(vec2 dest, mat3 m, vec2 v)
{
    dest[0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * 1;
    dest[1] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * 1;
    return;
}

void mat3_print(mat3 m, FILE *f)
{
    // Print row after row
    for(int y = 0; y < 3; y++)
    {
        fprintf(f, "(%f\t%f\t%f)\n", m[0][y], m[1][y], m[2][y]);
    }
    return;
}

//
// Vector
//

float vec2_dot(vec2 u, vec2 v)
{
    return u[0] * v[0] + u[1] * v[1];
}

float vec2_length(vec2 u)
{
    return sqrt(u[0]*u[0] + u[1]*u[1]);
}

void vec2_sub(vec2 dest, vec2 u, vec2 v)
{
    dest[0] = u[0] - v[0];
    dest[1] = u[1] - v[1];
    return;
}

void vec2_mult(vec2 dest, vec2 u, float k)
{
    dest[0] = u[0] * k;
    dest[1] = u[1] * k;
    return;
}

void vec2_print(vec2 v, FILE *f)
{
    fprintf(f, "(%f\t)\n(%f\t)\n", v[0], v[1]);
    return;
}
